#include "drone_serial_interface/protocol_handler.hpp"
#include <string>
#include <vector>
#include <cstdint>
#include <chrono>
#include <memory>
#include <algorithm>
#include <cstring>
#include <functional>
#include <sstream>
#include <cctype>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

namespace drone_serial_interface {

// ========== Base Protocol Handler ==========

ProtocolHandler::ProtocolHandler(const rclcpp::Logger & logger)
: logger_(logger)
{
}

// ========== Custom Protocol Handler ==========

CustomProtocolHandler::CustomProtocolHandler(const rclcpp::Logger & logger)
: ProtocolHandler(logger)
{
}

std::string CustomProtocolHandler::encode(const geometry_msgs::msg::Twist & cmd_vel)
{
  std::ostringstream oss;
  oss << "VEL," << cmd_vel.linear.x << "," << cmd_vel.linear.y << "," 
      << cmd_vel.linear.z << "," << cmd_vel.angular.z;
  
  uint32_t checksum = calculateChecksum(oss.str());
  oss << "," << checksum << "\n";
  
  return oss.str();
}

ProtocolStatus CustomProtocolHandler::validate(const std::string & encoded_data)
{
  ProtocolStatus status;
  status.timestamp = std::chrono::steady_clock::now();
  status.is_valid = false;
  
  // Basic validation
  if (encoded_data.empty() || encoded_data.back() != '\n') {
    status.error_message = "Invalid custom protocol format";
    return status;
  }
  
  status.is_valid = true;
  return status;
}

uint32_t CustomProtocolHandler::calculateChecksum(const std::string & data)
{
  uint32_t checksum = 0;
  for (char c : data) {
    checksum ^= static_cast<uint32_t>(static_cast<uint8_t>(c));
  }
  return checksum;
}

// ========== JSON Protocol Handler ==========

JsonProtocolHandler::JsonProtocolHandler(const rclcpp::Logger & logger)
: ProtocolHandler(logger)
{
}

std::string JsonProtocolHandler::encode(const geometry_msgs::msg::Twist & cmd_vel)
{
  std::ostringstream oss;
  oss << "{\"cmd\":\"velocity\",\"vx\":" << cmd_vel.linear.x 
      << ",\"vy\":" << cmd_vel.linear.y 
      << ",\"vz\":" << cmd_vel.linear.z 
      << ",\"wz\":" << cmd_vel.angular.z
      << ",\"ts\":" << getTimestampMs() << "}";
  
  return oss.str();
}

ProtocolStatus JsonProtocolHandler::validate(const std::string & encoded_data)
{
  ProtocolStatus status;
  status.timestamp = std::chrono::steady_clock::now();
  status.is_valid = false;
  
  // Basic JSON validation
  if (encoded_data.empty() || encoded_data.front() != '{' || encoded_data.back() != '}') {
    status.error_message = "Invalid JSON format";
    return status;
  }
  
  status.is_valid = true;
  return status;
}

uint32_t JsonProtocolHandler::calculateChecksum(const std::string & data)
{
  uint32_t checksum = 0;
  for (char c : data) {
    checksum ^= static_cast<uint32_t>(static_cast<uint8_t>(c));
  }
  return checksum;
}

uint64_t JsonProtocolHandler::getTimestampMs() const
{
  auto now = std::chrono::system_clock::now();
  auto duration = now.time_since_epoch();
  auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
  return static_cast<uint64_t>(millis);
}

// ========== Simple Protocol Handler ==========

SimpleProtocolHandler::SimpleProtocolHandler(const rclcpp::Logger & logger)
: ProtocolHandler(logger)
{
}

std::string SimpleProtocolHandler::encode(const geometry_msgs::msg::Twist & cmd_vel)
{
  std::ostringstream oss;
  oss << cmd_vel.linear.x << " " << cmd_vel.linear.y << " " 
      << cmd_vel.linear.z << " " << cmd_vel.angular.z << "\n";
  
  return oss.str();
}

ProtocolStatus SimpleProtocolHandler::validate(const std::string & encoded_data)
{
  ProtocolStatus status;
  status.timestamp = std::chrono::steady_clock::now();
  status.is_valid = false;
  
  // Basic validation
  if (encoded_data.empty() || encoded_data.back() != '\n') {
    status.error_message = "Invalid simple protocol format";
    return status;
  }
  
  status.is_valid = true;
  return status;
}

uint32_t SimpleProtocolHandler::calculateChecksum(const std::string & data)
{
  uint32_t checksum = 0;
  for (char c : data) {
    checksum ^= static_cast<uint32_t>(static_cast<uint8_t>(c));
  }
  return checksum;
}

// ========== Binary Protocol Handler ==========

BinaryProtocolHandler::BinaryProtocolHandler(const rclcpp::Logger & logger)
: ProtocolHandler(logger)
{
}

std::string BinaryProtocolHandler::encode(const geometry_msgs::msg::Twist & cmd_vel)
{
  std::vector<uint8_t> buffer;
  buffer.reserve(24);  // 6 * 4 bytes
  
  // Header
  writeUint32(buffer, BinaryProtocolHandler::HEADER_MAGIC);
  
  // Velocity data
  writeFloat32(buffer, static_cast<float>(cmd_vel.linear.x));
  writeFloat32(buffer, static_cast<float>(cmd_vel.linear.y));
  writeFloat32(buffer, static_cast<float>(cmd_vel.linear.z));
  writeFloat32(buffer, static_cast<float>(cmd_vel.angular.z));
  
  // Calculate checksum (simple XOR of all data)
  uint32_t checksum = 0;
  for (size_t i = 4; i < buffer.size(); ++i) {  // Skip header
    checksum ^= buffer[i];
  }
  writeUint32(buffer, checksum);
  
  // Footer
  writeUint32(buffer, BinaryProtocolHandler::FOOTER_MAGIC);
  
  // Convert to string
  return std::string(buffer.begin(), buffer.end());
}

ProtocolStatus BinaryProtocolHandler::validate(const std::string & encoded_data)
{
  ProtocolStatus status;
  status.timestamp = std::chrono::steady_clock::now();
  status.is_valid = false;
  
  if (encoded_data.size() != 28) {  // 7 * 4 bytes
    status.error_message = "Invalid binary data size: " + std::to_string(encoded_data.size());
    return status;
  }
  
  std::vector<uint8_t> buffer(encoded_data.begin(), encoded_data.end());
  
  // Check header
  uint32_t header = readUint32(buffer, 0);
  if (header != BinaryProtocolHandler::HEADER_MAGIC) {
    status.error_message = "Invalid header magic";
    return status;
  }
  
  // Check footer
  uint32_t footer = readUint32(buffer, 24);
  if (footer != BinaryProtocolHandler::FOOTER_MAGIC) {
    status.error_message = "Invalid footer magic";
    return status;
  }
  
  // Verify checksum
  uint32_t received_checksum = readUint32(buffer, 20);
  uint32_t calculated_checksum = 0;
  for (size_t i = 4; i < 20; ++i) {  // Data portion only
    calculated_checksum ^= buffer[i];
  }
  
  if (received_checksum != calculated_checksum) {
    status.error_message = "Binary checksum mismatch";
    return status;
  }
  
  status.checksum = calculated_checksum;
  status.is_valid = true;
  
  return status;
}

uint32_t BinaryProtocolHandler::calculateChecksum(const std::string & data)
{
  uint32_t checksum = 0;
  for (char c : data) {
    checksum ^= static_cast<uint32_t>(static_cast<uint8_t>(c));
  }
  return checksum;
}

void BinaryProtocolHandler::writeUint32(std::vector<uint8_t> & buffer, uint32_t value)
{
  buffer.push_back(static_cast<uint8_t>(value & 0xFF));
  buffer.push_back(static_cast<uint8_t>((value >> 8) & 0xFF));
  buffer.push_back(static_cast<uint8_t>((value >> 16) & 0xFF));
  buffer.push_back(static_cast<uint8_t>((value >> 24) & 0xFF));
}

void BinaryProtocolHandler::writeFloat32(std::vector<uint8_t> & buffer, float value)
{
  uint32_t int_value;
  std::memcpy(&int_value, &value, sizeof(float));
  writeUint32(buffer, int_value);
}

uint32_t BinaryProtocolHandler::readUint32(const std::vector<uint8_t> & buffer, size_t offset)
{
  return static_cast<uint32_t>(buffer[offset]) |
         (static_cast<uint32_t>(buffer[offset + 1]) << 8) |
         (static_cast<uint32_t>(buffer[offset + 2]) << 16) |
         (static_cast<uint32_t>(buffer[offset + 3]) << 24);
}

float BinaryProtocolHandler::readFloat32(const std::vector<uint8_t> & buffer, size_t offset)
{
  uint32_t int_value = readUint32(buffer, offset);
  float float_value;
  std::memcpy(&float_value, &int_value, sizeof(float));
  return float_value;
}

// ========== Protocol Factory ==========

std::unique_ptr<ProtocolHandler> ProtocolFactory::create(
  const std::string & protocol_name,
  const rclcpp::Logger & logger)
{
  std::string lower_name = protocol_name;
  std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
  
  if (lower_name == "custom") {
    return std::make_unique<CustomProtocolHandler>(logger);
  } else if (lower_name == "json") {
    return std::make_unique<JsonProtocolHandler>(logger);
  } else if (lower_name == "simple") {
    return std::make_unique<SimpleProtocolHandler>(logger);
  } else if (lower_name == "binary") {
    return std::make_unique<BinaryProtocolHandler>(logger);
  } else {
    RCLCPP_ERROR(logger, "Unknown protocol type: %s", protocol_name.c_str());
    return nullptr;
  }
}

std::vector<std::string> ProtocolFactory::getSupportedProtocols()
{
  return {"custom", "json", "simple", "binary"};
}

bool ProtocolFactory::isValidProtocol(const std::string & protocol_name)
{
  auto protocols = getSupportedProtocols();
  std::string lower_name = protocol_name;
  std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
  
  return std::find(protocols.begin(), protocols.end(), lower_name) != protocols.end();
}

} // namespace drone_serial_interface